#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  motorA,          lmotorA,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorB,          lmotorB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,          loader,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     dmotorA,       tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dmotorB,       tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     combine,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     basketmo,      tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    aim,                  tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    ramp,                 tServoNormal)
#pragma config(Servo,  srvo_S1_C3_3,    flpusher,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_4,    rlpusher,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_5,    basket,               tServoNormal)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//JoystickDreiver.c is the magic behind the getJoytickSettings(), and a few other functions;
#include "JoystickDriver.c"


//Keymap
#define LLAUNCHER_KEY joy2Btn(6)
#define LLAUNCHER_OFF_KEY joy2Btn(8)

#define BASKET_UP_KEY joy2Btn(4)
#define BASKET_DOWN_KEY joy2Btn(2)

#define COMBINE_KEY joy1Btn(6)
#define COMBINE_OFF_KEY joy1Btn(8)

#define RAMP_UP_KEY joy1Btn(5)
#define RAMP_DOWN_KEY joy1Btn(7)

#define BASKET_MOTOR_UP_KEY joy1Btn(1)

//#define AIM_UP_KEY joy2Btn(4)
//#define AIM_DOWN_KEY joy2Btn(2)

#define FLPUSHER_KEY joy2Btn(5)
#define RLPUSHER_KEY joy2Btn(7)

//Other misc. definitions
#define TELEOP_TIME 900
#define CONTROLLER_DEAD_ZONE 20

//Declare Functions For Project
//Init is where we'll put anything that needs to be done before the round starts. EX Put servos in a certain position,
//etc.
void init();

//drive is for the teleop commands
void drive();

// *Motors() is shorthand for motor[lmotorA] = ... etc
void launchMotors(short m1, short m2);
void driveMotors(short m1, short m2);
void combineMotor(short m);
void loaderMotor(short m);
void basketMotor(short m);

void init()
{
  servo[ramp] = 120;
  servo[aim] = 50;
  servo[flpusher] = 100;
  servo[rlpusher] = 180;
//Basket Up 4 Life
  basketMotor(100);
	servo[basket] = 130;
	wait10Msec(30);
	basketMotor(20);
}

task main()
{
  init();
  waitForStart();
  drive();
}

void drive()
{
  ClearTimer(T1);
  long dmotorAencoder, dmotorBencoder;

  while(time100[T1] <= TELEOP_TIME)
  {
	getJoystickSettings(joystick);
	dmotorAencoder = nMotorEncoder[dmotorA];
	dmotorBencoder = nMotorEncoder[dmotorB];

	//Launcher/Loader Controls
	if(LLAUNCHER_KEY)
	{
	   launchMotors(100,100);
	   loaderMotor(100);
	}
	else if (LLAUNCHER_OFF_KEY)
	{
       launchMotors(1,1);
	   loaderMotor(1);
	}

	//Combine Controls
	if(COMBINE_KEY)
		combineMotor(100);
	else if(COMBINE_OFF_KEY)
		combineMotor(1);

	//Basket Controls
	if(BASKET_UP_KEY)
	{
	  basketMotor(80);
	  servo[basket] = 130;
	  wait10Msec(30);
	  basketMotor(20);
	}
	else if(BASKET_DOWN_KEY)
	{
	  basketMotor(-1);
	  wait1Msec(1);
	  basketMotor(1);
	  servo[basket] = 80;
	}

	//Ramp Controls
	if(RAMP_UP_KEY)
	  servo[ramp] = 120;
	else if (RAMP_DOWN_KEY)
	  servo[ramp] = 0;

/*	//Launcher Aim Controls
	if(AIM_UP_KEY)
	  servo[aim] = 65;
	else if(AIM_DOWN_KEY)
	  servo[aim] = 50;
*/
	//Front T-Pusher Controls
	if(FLPUSHER_KEY)
	   servo[flpusher] = 230;
	else
	   servo[flpusher] = 100;

	 //Rear T-Pusher Controls
	if(RLPUSHER_KEY)
		servo[rlpusher] = 100;
	else
		servo[rlpusher] = 180;

	//Vroom Vroom
	driveMotors(joystick.joy1_y1, joystick.joy1_y2,);
  }
}

void launchMotors(short m1, short m2)
{
  motor[lmotorA] = m1;
  motor[lmotorB] = m2;
}

//Note that driveMotors will ignore values < CONTROLLER_DEAD_ZONE
void driveMotors(short m1, short m2)
{
  //Here we scale the joystick control values from (-127,127) to (-100,100) which is what the controller accepts.
  //There is also a deadzone of 10 either way to keep it stable. Due to an odd hardware glitch, we also have to
  //keep the motors at 1 power in order to keep them from misfiring
  if(abs(m1) >= CONTROLLER_DEAD_ZONE)
    motor[dmotorA] = m1/1.27;
  else
    motor[dmotorA] = 1;
  if(abs(m2) >= CONTROLLER_DEAD_ZONE)
    motor[dmotorB] = m2/1.27;
  else
    motor[dmotorB] = 1;
}

void combineMotor(short m)
{
  motor[combine] = m;
}

void loaderMotor(short m)
{
  motor[loader] = m;
}

void basketMotor(short m)
{
  motor[basketmo] = m;
}